#include<Servo.h>
#define PIN_SERV7 6 //舵机号（IO口）手抓
#define PIN_SERV8 9 //舵机号（IO口）前臂
#define PIN_SERV9 10 //舵机号（IO口）后臂
#define PIN_SERV10 11 //舵机号（IO口）  底座
#define input1 3 // 定义uno的pin 3 向 input1 输出
#define input2 4 // 定义uno的pin 4 向 input2 输出----//3、4引脚控制左轮
#define input3 5 // 定义uno的pin 5 向 input3 输出
#define input4 7 // 定义uno的pin 6 向 input4 输出----//5、7引脚控制右轮
Servo myservo_7, myservo_8, myservo_9, myservo_10;

char Forward()//前进
{
  digitalWrite(input1,HIGH);//给高电平1
  digitalWrite(input2, LOW); //给低电平0
  digitalWrite(input3, HIGH); //给高电平1
  digitalWrite(input4, LOW); //给低电平0
  while(1){
    if(Serial.available()){
      char mode = Serial.read();
      Stop();
      return mode;//返回新的指令
      }
}
}

char Backward() //后退
{
  digitalWrite(input1, LOW);
  digitalWrite(input2, HIGH);
  digitalWrite(input3, LOW);
  digitalWrite(input4, HIGH);
  while(1)
  {
    if(Serial.available())      // 如果要选择新的模式：逆时针转，停止
    {
        char mode = Serial.read();
        Stop();
        return mode;            // 返回新的指令
    }
   }
}

void Stop()//------------------停止
{
  digitalWrite(input1, LOW);
  digitalWrite(input2, LOW);
  digitalWrite(input3, LOW);
  digitalWrite(input4, LOW);
}

char  TurnRight()//-------------右转
{
  digitalWrite(input1, HIGH);
  digitalWrite(input2, LOW);
  digitalWrite(input3, LOW);
  digitalWrite(input4, HIGH);
  while(1)
  {
   
    if(Serial.available())      // 如果要选择新的模式：逆时针转，停止
    {
        char mode = Serial.read();
        Stop();
        return mode;            // 返回新的指令
    }
  }
}

char TurnLeft()//--------------左转
{
  digitalWrite(input1, LOW);
  digitalWrite(input2, HIGH);//左轮
  digitalWrite(input3, HIGH);//右轮
  digitalWrite(input4, LOW);
  while(1)
  {
    if(Serial.available())      // 如果要选择新的模式：逆时针转，停止
    {
        char mode = Serial.read();
        Stop();
        return mode;            // 返回新的指令
    }
  }
}

char Status(Servo myservo,int angle)//手 抓放  前后上下动作 左右动作 状态
{
   myservo.write(angle);
   return '9';
}

void setup()
{
  Serial.begin(9600);        //初始化串口
  pinMode(PIN_SERV7,OUTPUT);//设定舵机接口为输出接口  
  pinMode(PIN_SERV8,OUTPUT);
  pinMode(PIN_SERV9,OUTPUT);
  pinMode(PIN_SERV10,OUTPUT);

  myservo_7.attach(PIN_SERV7);// 将Servo变量附加到引脚，将7号引脚设置为7号舵机控制端口
  myservo_8.attach(PIN_SERV8);
  myservo_9.attach(PIN_SERV9); 
  myservo_10.attach(PIN_SERV10);

  pinMode(input1, OUTPUT);//小车串口设置为输出模式
  pinMode(input2, OUTPUT);
  pinMode(input3, OUTPUT);
  pinMode(input4, OUTPUT); 


  Serial.println("Test start ");
  myservo_7.write(150);           //使舵机转动至指定角度
  myservo_8.write(45);  
  myservo_9.write(45);  
  myservo_10.write(90);  
  Serial.println("Test OK! ");
}


void loop() {
  // put your main code here, to run repeatedly:
  if(Serial.available()>0){
    delay(100);
    char mode_ = Serial.read();
    switch(mode_){
      case '1':
         Serial.println("hand catch");
         mode_ = Status(myservo_7,180);
         break;
      case '2':
         Serial.println("hand release");
         mode_ = Status(myservo_7,120);
         break;
       case '3':
          Serial.println("qianbi qian");
          mode_ = Status(myservo_8,0);
          break;
       case '4':
          Serial.println("qianbi hou");
          mode_ = Status(myservo_8,90);
          break;
       case '5':
          Serial.println("houbi qian");
          mode_ = Status(myservo_9,0);
          break;
       case '6':
          Serial.println("houbi qian");
          mode_ = Status(myservo_9,90);
          break;
       case '7':
            Serial.println("zhengzhuan");
            mode_ = Status(myservo_10,0);
            break;
       case '8':
            Serial.println("fanzhuan");
            mode_ = Status(myservo_10,180);
            break;
        case '9':Serial.println("OK");break;
        case 'a':       //左转
              {
                mode_ = TurnLeft();
                Serial.write("turnleft");
                break;
              }
            case 'w':       //前进
              {
                mode_ = Forward();
                Serial.write("forward");
                break;
              }
            case 'd':       //右转
              {
                mode_ = TurnRight();
                Serial.write("turn right");
                break;
              }
            case 's':       //后退
              {
                mode_ = Backward();
                Serial.write("backward");
                break;
              }
            case '0':       //停止
              {
                Stop();
                Serial.write("stop");
                break;
              }
        default:;break;
    }   
  } 
}
